#pragma config(Hubs,  S1, HTMotor,  HTMotor,  none,     none)
#pragma config(Hubs,  S2, HTMotor,  HTMotor,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Sensor, S3,     seekerL,        sensorHiTechnicIRSeeker1200)
#pragma config(Sensor, S4,     seekerR,        sensorHiTechnicIRSeeker1200)
#pragma config(Motor,  mtr_S1_C1_1,     aux1,          tmotorTetrix, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     aux2,          tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     fl,            tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     fr,            tmotorTetrix, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S2_C1_1,     br,            tmotorTetrix, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S2_C1_2,     bl,            tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S2_C2_1,     ramp1,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C2_2,     ramp2,         tmotorTetrix, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"


void hold()                     //used to decrease error from momentum
{
	motor[br] = 0;
	motor[bl] = 0;
	motor[fr] = 0;
	motor[fl] = 0;
	wait1Msec(1200);
}

void rightSpeed(int power)      //used to simplify movement
{
	motor[fr]=power;
	motor[br]=power;
	return;
}
void leftSpeed(int power)         //used to simplify movement
{
	motor[fl]=power;
	motor[bl]=power;
	return;
}

void initalizeEncoders()
{
	nMotorEncoder[bl] = 0;
	nMotorEncoder[br] = 0;
	nMotorEncoder[fl] = 0;
	nMotorEncoder[fr] = 0;
	return;
}

void rightrung()                              //preprogramed method to navigate to right middle rung
{
	initalizeEncoders();

	nMotorEncoderTarget[bl] = 3050;       //forward
	nMotorEncoderTarget[br] = -3050;
	nMotorEncoderTarget[fl] = 3050;
	nMotorEncoderTarget[fr] = -3050;


	leftSpeed(50);
	rightSpeed(50);

	while(nMotorRunState[bl] != runStateIdle || nMotorRunState[br] != runStateIdle || nMotorRunState[fl] != runStateIdle || nMotorRunState[fr] != runStateIdle)
	{

	}

	hold();

	initalizeEncoders();



	nMotorEncoderTarget[bl] = 1150;    //turn
	nMotorEncoderTarget[br] = 1150;
	nMotorEncoderTarget[fl] = 1150;
	nMotorEncoderTarget[fr] = 1150;

	leftSpeed(50);
	rightSpeed(-50);

	while(nMotorRunState[bl] != runStateIdle || nMotorRunState[br] != runStateIdle || nMotorRunState[fl] != runStateIdle || nMotorRunState[fr] != runStateIdle)
	{

	}

	initalizeEncoders();

	nMotorEncoderTarget[bl] = 2150;       //forward
	nMotorEncoderTarget[br] = -2150;
	nMotorEncoderTarget[fl] = 2150;
	nMotorEncoderTarget[fr] = -2150;


	leftSpeed(50);
	rightSpeed(50);

	while(nMotorRunState[bl] != runStateIdle || nMotorRunState[br] != runStateIdle || nMotorRunState[fl] != runStateIdle || nMotorRunState[fr] != runStateIdle)
	{

	}

	initalizeEncoders();


	nMotorEncoderTarget[bl] = -2151;    //turn
	nMotorEncoderTarget[br] = -2151;
	nMotorEncoderTarget[fl] = -2151;
	nMotorEncoderTarget[fr] = -2151;

	leftSpeed(-50);
	rightSpeed(50);

	while(nMotorRunState[bl] != runStateIdle || nMotorRunState[br] != runStateIdle || nMotorRunState[fl] != runStateIdle || nMotorRunState[fr] != runStateIdle)
	{

	}


	hold();

	leftSpeed(0);
	rightSpeed(0);
	motor[aux1] = 50;
	motor[aux2] = 50;
	wait1Msec(1300);

	motor[aux1] = 0;
	motor[aux2] = 0;
	wait1Msec(300);


	hold();

	initalizeEncoders();

	nMotorEncoderTarget[bl] = 4800;
	nMotorEncoderTarget[br] = -4800;
	nMotorEncoderTarget[fl] = 4800;
	nMotorEncoderTarget[fr] = -4800;


	leftSpeed(50);
	rightSpeed(50);

	while(nMotorRunState[bl] != runStateIdle || nMotorRunState[br] != runStateIdle || nMotorRunState[fl] != runStateIdle || nMotorRunState[fr] != runStateIdle)
	{

	}

	hold();

	motor[aux1] = -50;
	motor[aux2] = -50;
	wait1Msec(300);

	motor[aux1] = 0;
	motor[aux2] = 0;
	wait1Msec(300);

	hold();

	nMotorEncoderTarget[bl] = -1440;
	nMotorEncoderTarget[br] = 1440;
	nMotorEncoderTarget[fl] = -1440;
	nMotorEncoderTarget[fr] = 1440;


	leftSpeed(-50);
	rightSpeed(-50);

	while(nMotorRunState[bl] != runStateIdle || nMotorRunState[br] != runStateIdle || nMotorRunState[fl] != runStateIdle || nMotorRunState[fr] != runStateIdle)
	{

	}

}

void midrung()                      //preprogramed method to navigate to middle middle rung
{
	initalizeEncoders();

	nMotorEncoderTarget[bl] = 3350;       //forward
	nMotorEncoderTarget[br] = -3350;
	nMotorEncoderTarget[fl] = 3350;
	nMotorEncoderTarget[fr] = -3350;


	leftSpeed(50);
	rightSpeed(50);

	while(nMotorRunState[bl] != runStateIdle || nMotorRunState[br] != runStateIdle || nMotorRunState[fl] != runStateIdle || nMotorRunState[fr] != runStateIdle)
	{

	}

	hold();

	initalizeEncoders();



	nMotorEncoderTarget[bl] = -1100;    //turn
	nMotorEncoderTarget[br] = -1100;
	nMotorEncoderTarget[fl] = -1100;
	nMotorEncoderTarget[fr] = -1100;

	leftSpeed(-50);
	rightSpeed(50);

	while(nMotorRunState[bl] != runStateIdle || nMotorRunState[br] != runStateIdle || nMotorRunState[fl] != runStateIdle || nMotorRunState[fr] != runStateIdle)
	{

	}

	leftSpeed(0);
	rightSpeed(0);
	motor[aux1] = 50;
	motor[aux2] = 50;
	wait1Msec(1700);

	motor[aux1] = 0;
	motor[aux2] = 0;
	wait1Msec(300);


	hold();

	initalizeEncoders();

	nMotorEncoderTarget[bl] = 4800;
	nMotorEncoderTarget[br] = -4800;
	nMotorEncoderTarget[fl] = 4800;
	nMotorEncoderTarget[fr] = -4800;


	leftSpeed(50);
	rightSpeed(50);

	while(nMotorRunState[bl] != runStateIdle || nMotorRunState[br] != runStateIdle || nMotorRunState[fl] != runStateIdle || nMotorRunState[fr] != runStateIdle)
	{

	}

	hold();

	motor[aux1] = -50;
	motor[aux2] = -50;
	wait1Msec(300);

	motor[aux1] = 0;
	motor[aux2] = 0;
	wait1Msec(300);

	hold();

	nMotorEncoderTarget[bl] = -1440;
	nMotorEncoderTarget[br] = 1440;
	nMotorEncoderTarget[fl] = -1440;
	nMotorEncoderTarget[fr] = 1440;


	leftSpeed(-50);
	rightSpeed(-50);

	while(nMotorRunState[bl] != runStateIdle || nMotorRunState[br] != runStateIdle || nMotorRunState[fl] != runStateIdle || nMotorRunState[fr] != runStateIdle)
	{

	}

	hold();


}

void leftrung()                               //preprogramed method to navigate to left middle rung
{
	initalizeEncoders();

	nMotorEncoderTarget[bl] = 350;       //forward
	nMotorEncoderTarget[br] = -350;
	nMotorEncoderTarget[fl] = 350;
	nMotorEncoderTarget[fr] = -350;


	leftSpeed(50);
	rightSpeed(50);

	while(nMotorRunState[bl] != runStateIdle || nMotorRunState[br] != runStateIdle || nMotorRunState[fl] != runStateIdle || nMotorRunState[fr] != runStateIdle)
	{

	}

	hold();

	initalizeEncoders();



	nMotorEncoderTarget[bl] = -1200;    //turn
	nMotorEncoderTarget[br] = -1200;
	nMotorEncoderTarget[fl] = -1200;
	nMotorEncoderTarget[fr] = -1200;

	leftSpeed(-50);
	rightSpeed(50);

	while(nMotorRunState[bl] != runStateIdle || nMotorRunState[br] != runStateIdle || nMotorRunState[fl] != runStateIdle || nMotorRunState[fr] != runStateIdle)
	{

	}

	leftSpeed(0);
	rightSpeed(0);
	motor[aux1] = 50;
	motor[aux2] = 50;
	wait1Msec(1350);

	motor[aux1] = 0;
	motor[aux2] = 0;
	wait1Msec(300);


	hold();

	initalizeEncoders();

	nMotorEncoderTarget[bl] = 7100;
	nMotorEncoderTarget[br] = -7100;
	nMotorEncoderTarget[fl] = 7100;
	nMotorEncoderTarget[fr] = -7100;


	leftSpeed(30);
	rightSpeed(30);

	while(nMotorRunState[bl] != runStateIdle || nMotorRunState[br] != runStateIdle || nMotorRunState[fl] != runStateIdle || nMotorRunState[fr] != runStateIdle)
	{

	}

	hold();

	motor[aux1] = -50;
	motor[aux2] = -50;
	wait1Msec(300);

	motor[aux1] = 0;
	motor[aux2] = 0;
	wait1Msec(300);

	hold();

	nMotorEncoderTarget[bl] = -1440;
	nMotorEncoderTarget[br] = 1440;
	nMotorEncoderTarget[fl] = -1440;
	nMotorEncoderTarget[fr] = 1440;


	leftSpeed(-50);
	rightSpeed(-50);

	while(nMotorRunState[bl] != runStateIdle || nMotorRunState[br] != runStateIdle || nMotorRunState[fl] != runStateIdle || nMotorRunState[fr] != runStateIdle)
	{

	}
}

task main()
{
	waitForStart();

	bDisplayDiagnostics=false;
	nxtDisplayCenteredTextLine(2, "Sensor Value: %d", SensorValue[seekerR]);    // Display "Sensor Value: ##"
	nxtDisplayCenteredTextLine(4, "Sensor Value: %d", SensorValue[seekerL]);    // Display "Sensor Value: ##"
	wait1Msec(1000);    // Wait 100 milliseconds to help display correctly.

	if((Sensorvalue(seekerL) == 6) && (sensorvalue(seekerR) == 7))
	{
		midrung();
	}

	else if(((Sensorvalue(seekerR) == 7)||(sensorvalue(seekerR) == 8))&& (sensorvalue(seekerL) == 7))
	{
		leftrung();
	}

	else if((Sensorvalue(seekerL) == 5) && (sensorvalue(seekerR) == 5))
	{
		rightrung();
	}
	else
	{
		midrung();
	}
}
